/**********************************************************************************************************
 * 函数功能：MPU6050初始化函数
 * 
 * 
 * *******************************************************************************************************/
#include "mpu6050.h"
#include "inv_mpu.h"
#include "stdio.h"
#include "my_i2c.h"

//初始化MPU6050
uint8_t MY_MPU6050_Init(void)
{
    uint8_t res = 0;
    MY_I2C_Master_Init();
    MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0x80);  //复位MPU6050
    vTaskDelay(100 / portTICK_PERIOD_MS);
    MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0x00);  //唤醒MPU6050
    MPU_Set_Gyro_FSR(3);   //陀螺仪传感器,±2000dps
    MPU_Set_Accel_FSR(0);  //加速度传感器,±2g
    MPU_Set_Rate(50);      //设置采样频率为50Hz
    MPU_Write_Byte(MPU_ADDR, MPU_INT_EN_REG, 0x00);  //关闭所有中断
    MPU_Write_Byte(MPU_ADDR, MPU_USER_CTRL_REG, 0x00);  //I2C主模式关闭
    MPU_Write_Byte(MPU_ADDR, MPU_FIFO_EN_REG, 0x00);   //FIFO关闭
    MPU_Write_Byte(MPU_ADDR, MPU_INTBP_CFG_REG, 0x80);  //INT引脚低电平有效,开启bypass模式，可以直接读取磁力计
    MPU_Read_Byte(MPU_ADDR, MPU_DEVICE_ID_REG, &res);  //读取器件ID
    if(res == MPU_ADDR)  //如果器件的ID正确
    {
        MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT1_REG, 0x01);  //设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_ADDR, MPU_PWR_MGMT2_REG, 0x00);  //加速度与陀螺仪都工作
        MPU_Set_Rate(50);  //设置采样频率为50Hz
    }
    else
    {
        return -1;
    }

    while(mpu_dmp_init())
    {
        vTaskDelay(20 / portTICK_PERIOD_MS);
    }

    return 0;
}

//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Gyro_FSR(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_ADDR, MPU_GYRO_CFG_REG, fsr << 3);
}

//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Accel_FSR(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_ADDR, MPU_ACCEL_CFG_REG, fsr << 3);
}

//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率（Hz）
//返回值：0，成功
//     其他：失败
uint8_t MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data = 0;
    if(data >= 188)
    {
        data = 1;
    }
    else if(lpf >= 98)
    {
        data = 2;
    }
    else if(lpf >= 42)
    {
        data = 3;
    }
    else if(lpf >= 20)
    {
        data = 4;
    }
    else if(lpf >= 10)
    {
        data = 5;
    }
    else
    {
        data = 6;
    }

    return MPU_Write_Byte(MPU_ADDR, MPU_CFG_REG, data);
}

//设置MPU6050的采样频率（假定Fs为1KHz）
//rate:(4~1000Hz)
//返回值：0，成功
//     其他，失败
uint8_t MPU_Set_Rate(uint16_t rate)
{
    uint8_t data;
    if(rate > 1000)
    {
        rate = 1000;
    }
    if(rate < 4)
    {
        rate = 4;
    }
    data = 1000 / rate - 1;
    data = MPU_Write_Byte(MPU_ADDR, MPU_SAMPLE_RATE_REG, data);  //设置数字低通滤波器

    return MPU_Set_LPF(rate / 2);  //LPF为采样率的一半
}

//读取MPU6050的温度
//返回值：温度值，实际扩大了100倍
uint16_t 
MPU_Get_Temperature(void)
{
    uint8_t buf[2];
    int16_t raw;
    float temp;
    if(MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf) != 0)
    {
        return -1;
    }
    raw = ((uint16_t)(buf[0] << 8)) | buf[1];
    temp = 36.53 + ((double)raw / 340);

    return temp * 100;
}

//得到陀螺仪的原始值
//gx,gy,gz分别是陀螺仪x轴，y轴和z轴的原始读数（带符号）
//返回值：0，成功
//    其他，错误
uint8_t MPU_Get_Gyroscope(int16_t *gx, int16_t *gy, int16_t *gz)
{
    uint8_t res;
    uint8_t buf[6];

    res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
    if(res == 0)
    {
        *gx = ((uint16_t)buf[0] << 8) | buf[1];
        *gy = ((uint16_t)buf[2] << 8) | buf[3];
        *gz = ((uint16_t)buf[4] << 8) | buf[5];
    }
    return res;
}

//得到加速度的原始值
uint16_t MPU_Get_Accelerometer(int16_t *ax, int16_t *ay, int16_t *az)
{
    uint8_t res;
    uint8_t buf[6];

    res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);
    if(res == 0)
    {
        *ax = ((uint16_t)buf[0] << 8) | buf[1];
        *ay = ((uint16_t)buf[2] << 8) | buf[3];
        *az = ((uint16_t)buf[4] << 8) | buf[5];
    }
    return res;
}

//I2C写一个字节
//reg：寄存器地址
//data：数据
//返回值0：正常
//    -1：错误代码
uint8_t MPU_Write_Byte(uint8_t addr, uint8_t reg, uint8_t data)
{
    //xSemaphoreTakeRecursive(I2C_Mutex,(TickType_t)100); 	//获取互斥信号量 阻塞时间设置为10
    MY_I2C_Write_Byte(addr, reg, data);
    //xSemaphoreGiveRecursive(I2C_Mutex) ;					//释放互斥信号量
    return 0;
}

//I2C连续写
//reg：寄存器地址
//len：写入长度
//buf：数据区
//返回值：0，正常
//     其他，错误
//uint8_t MPU_Write_Len(uint8_t reg, uint8_t *data, uint8_t len)
uint8_t MPU_Write_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *data)
{
    //xSemaphoreTakeRecursive(I2C_Mutex,(TickType_t)100);
    MY_I2C_Write_Len(addr, reg, len, data);
    //xSemaphoreGiveRecursive(I2C_Mutex);	
    return 0;
}

//I2C读一个字节
//reg:寄存器的地址
//返回值:0，成功
//    其他，错误
uint8_t MPU_Read_Byte(uint8_t addr, uint8_t reg, uint8_t *res)
{
    //xSemaphoreTakeRecursive(I2C_Mutex,(TickType_t)100);
    MY_I2C_Read_Byte(addr, reg, res);
    //xSemaphoreGiveRecursive(I2C_Mutex);
    return 0;
}

//I2C连续读
//reg：要读取的寄存器地址
//buf：要读取的数据存储区
//len：要读取的长度
//返回值：0，正常；其他，错误
//uint8_t MPU_Read_Len(uint8_t reg, uint8_t *buf, uint8_t len)
uint8_t MPU_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
    //xSemaphoreTakeRecursive(I2C_Mutex,(TickType_t)100);
    MY_I2C_Read_Len(addr, reg, len, buf);
    //xSemaphoreGiveRecursive(I2C_Mutex);
    return 0;
}

/* 初始化6050 */

/* 创建获取数据线程 */

/* 删除获取数据线程 */

